﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace GRIP
{
    public class FingerFinder2
    {
        struct RingPoint
        {
            public int x;
            public int y;
            public int index;
            public double angle;
        }

        private RingPoint[][] RingPoints;
        private const int DepthThreshold = 60;
        private const int FingertipThreshold = 10;

        public FingerFinder2(int Min, int Max, int Steps)
        {
            RingPoints = new RingPoint[Steps][];
            double interval = (float)(Max - Min) / (float)(Steps - 1);

            for (int i = 0; i < Steps; i++)
            {
                int radius = (int)(Math.Round(Min + interval * (Steps-1-i)));
                RingPoints[i] = ConstructRing(radius);
            }

        }

        // Shamelessly copied and translated from Wikipedia
        // http://en.wikipedia.org/wiki/Midpoint_circle_algorithm
        private RingPoint[] ConstructRing(int radius)
        {
            List<RingPoint> newRingPoints;

            List<RingPoint> octet1_Points = new List<RingPoint>();
            List<RingPoint> octet2_Points = new List<RingPoint>();
            List<RingPoint> octet3_Points = new List<RingPoint>();
            List<RingPoint> octet4_Points = new List<RingPoint>();
            List<RingPoint> octet5_Points = new List<RingPoint>();
            List<RingPoint> octet6_Points = new List<RingPoint>();
            List<RingPoint> octet7_Points = new List<RingPoint>();
            List<RingPoint> octet8_Points = new List<RingPoint>();

            octet1_Points.Add(CreatePoint(0, radius));
            octet3_Points.Add(CreatePoint(radius, 0));
            octet5_Points.Add(CreatePoint(0, -radius));
            octet7_Points.Add(CreatePoint(-radius, 0));

            int f = 1 - radius;
            int ddF_x = 1;
            int ddF_y = -2 * radius;
            int x = 0;
            int y = radius;

            while (x < y)
            {
                if (f >= 0)
                {
                    y--;
                    ddF_y += 2;
                    f += ddF_y;
                }
                x++;
                ddF_x += 2;
                f += ddF_x;

                octet1_Points.Add(CreatePoint(x, y));
                octet2_Points.Add(CreatePoint(y, x));
                octet3_Points.Add(CreatePoint(y, -x));
                octet4_Points.Add(CreatePoint(x, -y));
                octet5_Points.Add(CreatePoint(-x, -y));
                octet6_Points.Add(CreatePoint(-y, -x));
                octet7_Points.Add(CreatePoint(-y, x));
                octet8_Points.Add(CreatePoint(-x, y));
            }

            octet2_Points.Reverse();
            octet4_Points.Reverse();
            octet6_Points.Reverse();
            octet8_Points.Reverse();

            newRingPoints = new List<RingPoint>(octet1_Points.Count * 8 + 8);
            newRingPoints.AddRange(octet1_Points);
            newRingPoints.AddRange(octet2_Points);
            newRingPoints.AddRange(octet3_Points);
            newRingPoints.AddRange(octet4_Points);
            newRingPoints.AddRange(octet5_Points);
            newRingPoints.AddRange(octet6_Points);
            newRingPoints.AddRange(octet7_Points);
            newRingPoints.AddRange(octet8_Points);

            return newRingPoints.ToArray();
        }

        private RingPoint CreatePoint(int x, int y)
        {
            double angle = 180.0 / Math.PI * Math.Atan2(y, x);
            angle = angle < 0 ? angle + 360 : angle;
            return new RingPoint
            {
                x = x,
                y = y,
                index = y * 640 + x,
                angle = angle
            };
        }

        public List<System.Windows.Point> FindFingers(short[] DepthValues, int handX, int handY, int wristX, int wristY)
        {
            // use % to wrap around

            short referenceDepth = DepthValues[handY * 640 + handX];
            int indexOffset = 640 * handY + handX;

            List<List<RingPoint>> Lines = new List<List<RingPoint>>(5);
            bool tracking = false;

            for (int i = 0; i < RingPoints.Length; i++)
            {
                List<RingPoint> FingerPoints = new List<RingPoint>();
                RingPoint[] currentRingIndices = RingPoints[i];
                List<RingPoint> currentFingerSet = new List<RingPoint>();

                for (int j = 0; j < currentRingIndices.Length; j++)
                {
                    RingPoint currentPoint = currentRingIndices[j];
                    int index = currentPoint.index + indexOffset;
                    int x = currentPoint.x + handX;
                    int y = currentPoint.y + handY;
                    currentPoint = new RingPoint
                    {
                        x = x,
                        y = y,
                        index = index,
                        angle = currentPoint.angle
                    };

                    if (y > -1 && y < 480 && x > -1 && x < 640)
                    {
                        //FingerPoints.Add(index);

                        short depth = DepthValues[index];
                        if (depth != 0)
                        {
                            if (tracking)
                            {
                                //FingerPoints.Add(index);
                                currentFingerSet.Add(currentPoint);
                                if (Math.Abs(depth - referenceDepth) > DepthThreshold)
                                {
                                    tracking = false;
                                    if (currentFingerSet.Count > 2 && currentFingerSet.Count < 20)
                                        FingerPoints.Add(currentFingerSet[currentFingerSet.Count / 2]);
                                    //FingerPoints.AddRange(currentFingerSet);
                                    currentFingerSet.Clear();
                                }
                            }
                            else
                            {
                                if (Math.Abs(depth - referenceDepth) < DepthThreshold)
                                {
                                    currentFingerSet.Add(currentPoint);
                                    tracking = true;
                                }
                            }
                        }
                        else
                        {
                            if (tracking)
                            {
                                if (currentFingerSet.Count > 2 && currentFingerSet.Count < 20)
                                    FingerPoints.Add(currentFingerSet[currentFingerSet.Count / 2]);
                                //FingerPoints.AddRange(currentFingerSet);
                                currentFingerSet.Clear();
                            }
                            tracking = false;
                        }
                    }
                }

                // line indexing goes here
                for (int k = 0; k < Lines.Count; k++)
                {
                    List<RingPoint> line = Lines[k];
                    RingPoint lastPoint = line[line.Count - 1];
                    RingPoint minPoint = lastPoint;
                    double min = 1000000;
                    int minIndex = -1;
                    for (int l = 0; l < FingerPoints.Count; l++)
                    {
                        RingPoint currentPoint = FingerPoints[l];
                        double delta = Math.Abs(lastPoint.angle - currentPoint.angle);
                        if (delta < min && delta < 6)
                        {
                            min = delta;
                            minPoint = currentPoint;
                            minIndex = l;
                        }
                    }
                    if (minIndex > -1)
                    {
                        FingerPoints.RemoveAt(minIndex);
                        line.Add(minPoint);
                    }
                }

                for (int l = 0; l < FingerPoints.Count; l++)
                {
                    List<RingPoint> line = new List<RingPoint>();
                    line.Add(FingerPoints[l]);
                    Lines.Add(line);
                }
            }


            double handAngle = 180.0 / Math.PI * Math.Atan2(wristY - handY, wristX - handX);
            handAngle = handAngle < 0 ? handAngle + 360 : handAngle;

            List<System.Windows.Point> fingers = new List<System.Windows.Point>(5);
            List<RingPoint> fingerPoints = new List<RingPoint>(Lines.Count);

            // Recalculate angles of fingertips in terms of hand/wrist vector
            for (int i = 0; i < Lines.Count; i++)
            {
                List<RingPoint> curList = Lines[i];
                if (curList.Count > 2)
                {
                    RingPoint fingerPoint = curList[0];
                    double lineAngle = fingerPoint.angle - handAngle;
                    fingerPoint.angle = lineAngle < 0 ? lineAngle + 360 : lineAngle;
                    fingerPoints.Add(fingerPoint);
                }
            }

            fingerPoints.Sort(new FingerComparer());

            for (int i = 0; i < fingerPoints.Count; i++)
            {
                RingPoint finger = fingerPoints[i];
                if (finger.angle > 45 && finger.angle < 315)
                {
                    int xdelta = finger.x - handX;
                    int ydelta = finger.y - handY;
                    double dist = xdelta * xdelta + ydelta * ydelta;
                    if(dist > 400)
                        fingers.Add(new System.Windows.Point(finger.x, finger.y));
                }
            }

            /*for (int i = 0; i < Lines.Count; i++)
            {
                List<RingPoint> curList = Lines[i];
                if (curList.Count > 2)
                {
                    double lineAngle = curList[0].angle;
                    lineAngle = lineAngle < 0 ? lineAngle + 360 : lineAngle;
                    List<System.Windows.Point> list = new List<System.Windows.Point>();
                    for (int j = 0; j < curList.Count; j++)
                        list.Add(new System.Windows.Point(curList[j].x, curList[j].y));
                    fingers.Add(list);
                }
            }*/
            /*
            for (int i = 0; i < Lines.Count; i++)
            {
                List<RingPoint> line = Lines[i];
                if(line.Count > 8)
                    fingers.Add(Lines[i][0].index);
            }
             * */
            return fingers;
        }

        private class FingerComparer : IComparer<RingPoint>
        {
            public int Compare(RingPoint a, RingPoint b)
            {
                return a.angle.CompareTo(b.angle);
            }
        }
    }

}
